#include <cstdio>
#include <cstring>

#include "Skeleton.hpp"
#include <glm/gtc/matrix_transform.hpp>


void Skeleton::load(const char *name)
{
  FILE *fd;  
  fd = fopen( name, "r");
  
  fscanf( fd, "%u\n", &m_jointCount);
  
  m_joints            = new Joint_t       [ m_jointCount ];
  m_lerpKF.localPoses = new JointPose_t   [ m_jointCount ];
  m_animGlobalPos     = new aer::Matrix4x4[ m_jointCount ];  
  AER_CHECK( m_joints && m_lerpKF.localPoses && m_animGlobalPos );


  // for debugging
  m_dbgGlobalBind = new aer::Matrix4x4[ m_jointCount ]; 
  AER_CHECK( m_dbgGlobalBind );
  
  
  // =================
  // Load bones
  // =================
  aer::Quaternion quat;
  aer::Vector3    vec;
  for (int i=0; i<m_jointCount; ++i)
  {
    Joint_t &Jt = m_joints[i];
    
    fscanf( fd, "%s %d %f %f %f %f %f %f %f\n", Jt.name, &Jt.parentId,
                                                &quat.w, &quat.x, &quat.y, &quat.z,
                                                &vec.x, &vec.y, &vec.z);

    aer::Matrix4x4 rotation = glm::mat4_cast( quat );
    
    // Local bind
    m_dbgGlobalBind[i] = glm::translate( glm::mat4(1.0f), vec) * rotation;
    
    // Inverse bind
    Jt.invBindPoseMatrix = glm::transpose( rotation ) * glm::translate( glm::mat4(1.0f), -vec);
        
    if (i > 0) 
    {
      // global bind
      m_dbgGlobalBind[i] = m_dbgGlobalBind[Jt.parentId] * m_dbgGlobalBind[i];
      
      // inverse bind
      Jt.invBindPoseMatrix *= m_joints[Jt.parentId].invBindPoseMatrix;
    }
  }
  
    
  // =================
  // Load anims
  // =================
  fscanf( fd, "%u\n", &m_sequenceCount);
  
  m_sequences = new Sequence_t[ m_sequenceCount ];
  
  for (int i=0; i<m_sequenceCount; ++i) 
  {
    Sequence_t &sequence = m_sequences[i];

    unsigned int samplesTimesJointCount = 0;
    fscanf( fd, "%s %f %u", sequence.name, &sequence.framesPerSecond, &samplesTimesJointCount);
    
    sequence.frameCount = samplesTimesJointCount / m_jointCount;
    sequence.keyframes = new Keyframe_t[ sequence.frameCount ];
    
    for (int j=0; j<sequence.frameCount; ++j) 
    {
      Keyframe_t &kf = sequence.keyframes[j];
      kf.localPoses = new JointPose_t[ m_jointCount ];
      
      for (int k=0; k<m_jointCount; ++k) 
      {
        JointPose_t &joint = kf.localPoses[k];
        fscanf( fd, "%f %f %f %f ", &joint.qRotation.w, &joint.qRotation.x, 
                                    &joint.qRotation.y, &joint.qRotation.z);
        fscanf( fd, "%f %f %f ",    &joint.vTranslation.x, &joint.vTranslation.y, 
                                    &joint.vTranslation.z);
        joint.fScale = 1.0f;
      }
    }
  }
  fclose( fd );
}


void Skeleton::computePose( const aer::uint sequence_id, 
                            const float anim_startTime,
                            aer::Matrix4x4 *skinningMatrices)
{
  static float staticF = 0.0f;
  staticF += 1.0f;
  
  // [DEBUG] external params
  const float AnimationRate = 1.0f;
  const float AnimationStartTime = anim_startTime;
  const float globalTime = 0.01f*staticF; // 0.7f;

  // [TMP]
  if (skinningMatrices == NULL) {
    skinningMatrices = new aer::Matrix4x4[m_jointCount];
  }
  
 
  AER_ASSERT( sequence_id < m_sequenceCount );
  
  const Sequence_t &sequence = m_sequences[sequence_id];  
  const float SequenceDuration = sequence.frameCount / float(sequence.framesPerSecond);
  
  float localTime = AnimationRate * (globalTime - AnimationStartTime);

  if (sequence.bIsLooping) {
    localTime = fmod( localTime, SequenceDuration);
  } else {
    localTime = glm::clamp( localTime, 0.0f, SequenceDuration);
  }
  
  aer::F32 lerpFrame = localTime * sequence.framesPerSecond;
  
  aer::U32 frame = aer::U32(lerpFrame);
  aer::F32 lerpFactor = lerpFrame - frame;
  
  Keyframe_t *pKF = &sequence.keyframes[ frame ];
  
  // Retrieve the interpolated Keyframe if needed
  if (lerpFactor > 0.0f)
  { 
    const Keyframe_t &Kf1 = sequence.keyframes[ frame ];
    const Keyframe_t &Kf2 = sequence.keyframes[ (frame+1)%sequence.frameCount ];
    
    for(aer::U32 i=0; i<m_jointCount; ++i)
    {
      JointPose_t &joint = m_lerpKF.localPoses[i];
      const JointPose_t &J1 = Kf1.localPoses[i];
      const JointPose_t &J2 = Kf2.localPoses[i];
      
      joint.qRotation    = glm::mix( J1.qRotation,    J2.qRotation,    lerpFactor);
      joint.vTranslation = glm::mix( J1.vTranslation, J2.vTranslation, lerpFactor);
      joint.fScale       = glm::mix( J1.fScale,       J2.fScale,       lerpFactor);
    }
    
    pKF = &m_lerpKF;
  }
  
  // Generate the skinning matrices
  for(aer::U32 i=0; i<m_jointCount; ++i)
  {
    const JointPose_t &joint = pKF->localPoses[i];
        
    // Compute the keyframe local matrix    
    aer::Matrix4x4 animLocalPos;
    m_animGlobalPos[i]  = glm::translate( glm::mat4(1.0f), joint.vTranslation);
    m_animGlobalPos[i] *= glm::mat4_cast( joint.qRotation );
    
    // Multiply the non root bones with their parent
    if (i > 0) {
      //m_animGlobalPos[i] = m_animGlobalPos[i] * m_animGlobalPos[ m_joints[i].parentId ];
      m_animGlobalPos[i] = m_animGlobalPos[ m_joints[i].parentId ] * m_animGlobalPos[i];
    }
    
    skinningMatrices[i] = m_joints[i].invBindPoseMatrix * m_animGlobalPos[i];
  }
}


#if 0

  ---------
  
  + Pour chaque ActiveSequence
      Calculer la pose pour la frame courante (interpolée)
      output: n buffers de JointPose_t
    
  + If more than one ActiveSequence_t
      Blend between them
      output: 1 buffer de JointPose_t
      
  ---------
  
  
  + [Post-processing]  
  + Generate the global pose  
  + [Post-processing]
  
  
  + Generate the buffer of skinning matrix

#endif
